using System;
using System.Runtime.CompilerServices;
#if MYCODE
using BepuUtilities.Vectors;
#else
using System.Numerics;
#endif

namespace BepuUtilities
{
    /// <summary>
    /// 为四元数提供附加功能和一些开销较低的函数变体。
    /// </summary>
    public static class QuaternionEx
    {
        /// <summary>
        /// 将两个四元数相加。
        /// </summary>
        /// <param name="a">要添加的第一个四元数。</param>
        /// <param name="b">要添加的第二个四元数。</param>
        /// <param name="result">加法的总和。</param>
        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public static void Add(in Quaternion a, in Quaternion b, out Quaternion result)
        {
            result.X = a.X + b.X;
            result.Y = a.Y + b.Y;
            result.Z = a.Z + b.Z;
            result.W = a.W + b.W;
        }

        /// <summary>
        /// 缩放四元数。
        /// </summary>
        /// <param name="q">要相乘的四元数。</param>
        /// <param name="scale">将四元数的每个分量乘以。</param>
        /// <param name="result">缩放四元数。</param>
        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public static void Scale(in Quaternion q, float scale, out Quaternion result)
        {
            result.X = q.X * scale;
            result.Y = q.Y * scale;
            result.Z = q.Z * scale;
            result.W = q.W * scale;
        }

        /// <summary>
        /// 将两个四元数的变换串联在一起,以使作为向量v的方向应用的结果四元数等效于
        /// 变换=(v*a)*b。
        /// 假设两个输入参数都不与输出参数重叠。
        /// </summary>
        /// <param name="a">要串联的第一个四元数。</param>
        /// <param name="b">要串联的第二个四元数。</param>
        /// <param name="result">串联的乘积。</param>
        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public static void ConcatenateWithoutOverlap(in Quaternion a, in Quaternion b, out Quaternion result)
        {
            result.X = a.W * b.X + a.X * b.W + a.Z * b.Y - a.Y * b.Z;
            result.Y = a.W * b.Y + a.Y * b.W + a.X * b.Z - a.Z * b.X;
            result.Z = a.W * b.Z + a.Z * b.W + a.Y * b.X - a.X * b.Y;
            result.W = a.W * b.W - a.X * b.X - a.Y * b.Y - a.Z * b.Z;
        }

        /// <summary>
        /// 将两个四元数的变换串联在一起,以使作为向量v的方向应用的结果四元数等效于
        /// 变换=(v*a)*b。
        /// </summary>
        /// <param name="a">要串联的第一个四元数。</param>
        /// <param name="b">要串联的第二个四元数。</param>
        /// <param name="result">串联的乘积。</param>
        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public static void Concatenate(in Quaternion a, in Quaternion b, out Quaternion result)
        {
            ConcatenateWithoutOverlap(a, b, out var temp);
            result = temp;
        }


        /// <summary>
        /// 将两个四元数的变换串联在一起,以使作为向量v的方向应用的结果四元数等效于
        /// 变换=(v*a)*b。
        /// </summary>
        /// <param name="a">要相乘的第一个四元数。</param>
        /// <param name="b">要相乘的第二个四元数。</param>
        /// <return>乘法的乘积。</return>
        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public static Quaternion Concatenate(in Quaternion a, in Quaternion b)
        {
            ConcatenateWithoutOverlap(a, b, out var result);
            return result;
        }

        /// <summary>
        /// 表示单位变换的四元数。
        /// </summary>
        public static Quaternion Identity
        {
            [MethodImpl(MethodImplOptions.AggressiveInlining)]
            get
            {
                return new Quaternion(0, 0, 0, 1);
            }
        }


        /// <summary>
        /// 从旋转矩阵构造一个四元数。
        /// </summary>
        /// <param name="r">要从中创建四元数的旋转矩阵。</param>
        /// <param name="q">基于旋转矩阵的四元数。</param>
        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public static void CreateFromRotationMatrix(in Matrix3x3 r, out Quaternion q)
        {
            float t;
            if (r.Z.Z < 0)
            {
                if (r.X.X > r.Y.Y)
                {
                    t = 1 + r.X.X - r.Y.Y - r.Z.Z;
                    q.X = t;
                    q.Y = r.X.Y + r.Y.X;
                    q.Z = r.Z.X + r.X.Z;
                    q.W = r.Y.Z - r.Z.Y;
                }
                else
                {
                    t = 1 - r.X.X + r.Y.Y - r.Z.Z;
                    q.X = r.X.Y + r.Y.X;
                    q.Y = t;
                    q.Z = r.Y.Z + r.Z.Y;
                    q.W = r.Z.X - r.X.Z;
                }
            }
            else
            {
                if (r.X.X < -r.Y.Y)
                {
                    t = 1 - r.X.X - r.Y.Y + r.Z.Z;
                    q.X = r.Z.X + r.X.Z;
                    q.Y = r.Y.Z + r.Z.Y;
                    q.Z = t;
                    q.W = r.X.Y - r.Y.X;
                }
                else
                {
                    t = 1 + r.X.X + r.Y.Y + r.Z.Z;
                    q.X = r.Y.Z - r.Z.Y;
                    q.Y = r.Z.X - r.X.Z;
                    q.Z = r.X.Y - r.Y.X;
                    q.W = t;
                }
            }
            Scale(q, 0.5f / (float)Math.Sqrt(t), out q);
        }

        /// <summary>
        /// 从旋转矩阵创建四元数。
        /// </summary>
        /// <param name="r">用于创建新四元数的旋转矩阵。</param>
        /// <return>表示与矩阵相同旋转的四元数。</return>
        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public static Quaternion CreateFromRotationMatrix(in Matrix3x3 r)
        {
            CreateFromRotationMatrix(r, out var toReturn);
            return toReturn;
        }


        /// <summary>
        /// 从旋转矩阵构造一个四元数。
        /// </summary>
        /// <param name="r">要从中创建四元数的旋转矩阵。</param>
        /// <param name="q">基于旋转矩阵的四元数。</param>
        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public static void CreateFromRotationMatrix(in Matrix r, out Quaternion q)
        {
            Matrix3x3.CreateFromMatrix(r, out var rotation3x3);
            CreateFromRotationMatrix(rotation3x3, out q);
        }

        /// <summary>
        /// 从旋转矩阵构造一个四元数。
        /// </summary>
        /// <param name="r">要从中创建四元数的旋转矩阵。</param>
        /// <return>基于旋转矩阵的四元数。</return>
        public static Quaternion CreateFromRotationMatrix(in Matrix r)
        {
            Matrix3x3.CreateFromMatrix(r, out var rotation3x3);
            CreateFromRotationMatrix(rotation3x3, out var q);
            return q;
        }

        /// <summary>
        /// 确保四元数具有单位长度。
        /// </summary>
        /// <param name="四元数">要规格化的四元数。</param>
        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public static void Normalize(ref Quaternion quaternion)
        {
            ref var q = ref Unsafe.As<Quaternion, Vector4>(ref quaternion);
            q /= (float)Math.Sqrt(Vector4.Dot(q,q));//不太好;MathF何时可用？
        }

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public static Quaternion Normalize(Quaternion quaternion)
        {
            Normalize(ref quaternion);
            return quaternion;
        }

        /// <summary>
        /// 计算四元数的平方长度。
        /// </summary>
        /// <return>四元数长度的平方。</return>
        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public static float LengthSquared(ref Quaternion quaternion)
        {
            return Unsafe.As<Quaternion, Vector4>(ref quaternion).LengthSquared();
        }

        /// <summary>
        /// 计算四元数的长度。
        /// </summary>
        /// <return>四元数的长度。</return>
        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public static float Length(ref Quaternion quaternion)
        {
            return Unsafe.As<Quaternion, Vector4>(ref quaternion).Length();
        }

        /// <summary>
        /// 将两个四元数混合在一起以获得中间状态。
        /// </summary>
        /// <param name="start">插值的起点。</param>
        /// <param name="end">插值的终点。</param>
        /// <param name="interpolationAmount">要使用的终结点数量。</param>
        /// <param name="result">插值中间四元数。</param>
        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public static void Slerp(in Quaternion start, Quaternion end, float interpolationAmount, out Quaternion result)
        {
            double cosHalfTheta = start.W * end.W + start.X * end.X + start.Y * end.Y + start.Z * end.Z;
            if (cosHalfTheta < 0)
            {
                // 对四元数求反会导致相同的方向,
                // 但是我们需要cosHalfTheta为正值才能得到最短路径。
                end.X = -end.X;
                end.Y = -end.Y;
                end.Z = -end.Z;
                end.W = -end.W;
                cosHalfTheta = -cosHalfTheta;
            }
            // 如果方向足够相似,那么只需选择其中一个输入即可。
            if (cosHalfTheta > (1.0 - 1e-12))
            {
                result.W = start.W;
                result.X = start.X;
                result.Y = start.Y;
                result.Z = start.Z;
                return;
            }
            // 计算临时值。
            double halfTheta = Math.Acos(cosHalfTheta);
            double sinHalfTheta = Math.Sqrt(1.0 - cosHalfTheta * cosHalfTheta);

            double aFraction = Math.Sin((1 - interpolationAmount) * halfTheta) / sinHalfTheta;
            double bFraction = Math.Sin(interpolationAmount * halfTheta) / sinHalfTheta;

            // 将两个四元数混合得到结果！
            result.X = (float)(start.X * aFraction + end.X * bFraction);
            result.Y = (float)(start.Y * aFraction + end.Y * bFraction);
            result.Z = (float)(start.Z * aFraction + end.Z * bFraction);
            result.W = (float)(start.W * aFraction + end.W * bFraction);
        }

        /// <summary>
        /// 将两个四元数混合在一起以获得中间状态。
        /// </summary>
        /// <param name="start">插值的起点。</param>
        /// <param name="end">插值的终点。</param>
        /// <param name="interpolationAmount">要使用的终结点数量。</param>
        /// <return>插值的中间四元数。</return>
        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public static Quaternion Slerp(in Quaternion start, in Quaternion end, float interpolationAmount)
        {
            Slerp(start, end, interpolationAmount, out Quaternion toReturn);
            return toReturn;
        }


        /// <summary>
        /// 计算四元数的共轭。
        /// </summary>
        /// <param name="四元数">要共轭的四元数。</param>
        /// <param name="result">共轭四元数。</param>
        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public static void Conjugate(in Quaternion quaternion, out Quaternion result)
        {
            result.X = -quaternion.X;
            result.Y = -quaternion.Y;
            result.Z = -quaternion.Z;
            result.W = quaternion.W;
        }

        /// <summary>
        /// 计算四元数的共轭。
        /// </summary>
        /// <param name="四元数">要共轭的四元数。</param>
        /// <return>共轭四元数。</return>
        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public static Quaternion Conjugate(in Quaternion quaternion)
        {
            Conjugate(quaternion, out Quaternion toReturn);
            return toReturn;
        }



        /// <summary>
        /// 计算四元数的逆。
        /// </summary>
        /// <param name="四元数">要反转的四元数。</param>
        /// <param name="result">倒置的结果。</param>
        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public static void Inverse(in Quaternion quaternion, out Quaternion result)
        {
            float inverseSquaredNorm = quaternion.X * quaternion.X + quaternion.Y * quaternion.Y + quaternion.Z * quaternion.Z + quaternion.W * quaternion.W;
            result.X = -quaternion.X * inverseSquaredNorm;
            result.Y = -quaternion.Y * inverseSquaredNorm;
            result.Z = -quaternion.Z * inverseSquaredNorm;
            result.W = quaternion.W * inverseSquaredNorm;
        }

        /// <summary>
        /// 计算四元数的逆。
        /// </summary>
        /// <param name="四元数">要反转的四元数。</param>
        /// <return>反转的结果。</return>
        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public static Quaternion Inverse(in Quaternion quaternion)
        {
            Inverse(quaternion, out var result);
            return result;

        }

        /// <summary>
        /// 求反四元数的分量。
        /// </summary>
        /// <param name="a">要求反的四元数。</param>
        /// <param name="b">否定的结果。</param>
        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public static void Negate(in Quaternion a, out Quaternion b)
        {
            b.X = -a.X;
            b.Y = -a.Y;
            b.Z = -a.Z;
            b.W = -a.W;
        }

        /// <summary>
        /// 求反四元数的分量。
        /// </summary>
        /// <param name="q">要求反的四元数。</param>
        /// <Returns>否定的结果。</Returns>
        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public static Quaternion Negate(in Quaternion q)
        {
            Negate(q, out var result);
            return result;
        }

        /// <summary>
        /// 假设输出没有与输入产生锯齿,则使用四元数变换向量。
        /// </summary>
        /// <param name="v">要转换的向量。</param>
        /// <param name="Rotation">应用于向量的旋转。</param>
        /// <param name="result">转换后的向量。</param>
        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public static void TransformWithoutOverlap(in Vector3 v, in Quaternion rotation, out Vector3 result)
        {
            // 此运算是v‘=q*v*q^-1的优化向下版本。
            // 展开后的形式将是将v视为"仅轴"四元数。
            // 并执行标准的四元数乘法。假设Q是归一化的,
            // q^-1可以用共轭来代替。
            float x2 = rotation.X + rotation.X;
            float y2 = rotation.Y + rotation.Y;
            float z2 = rotation.Z + rotation.Z;
            float xx2 = rotation.X * x2;
            float xy2 = rotation.X * y2;
            float xz2 = rotation.X * z2;
            float yy2 = rotation.Y * y2;
            float yz2 = rotation.Y * z2;
            float zz2 = rotation.Z * z2;
            float wx2 = rotation.W * x2;
            float wy2 = rotation.W * y2;
            float wz2 = rotation.W * z2;
            // 延迟组件设置,因为它们在计算中使用。
            result.X = v.X * (1f - yy2 - zz2) + v.Y * (xy2 - wz2) + v.Z * (xz2 + wy2);
            result.Y = v.X * (xy2 + wz2) + v.Y * (1f - xx2 - zz2) + v.Z * (yz2 - wx2);
            result.Z = v.X * (xz2 - wy2) + v.Y * (yz2 + wx2) + v.Z * (1f - xx2 - yy2);
        }


        /// <summary>
        /// 使用四元数变换向量。
        /// </summary>
        /// <param name="v">要转换的向量。</param>
        /// <param name="Rotation">应用于向量的旋转。</param>
        /// <param name="result">转换后的向量。</param>
        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public static void Transform(in Vector3 v, in Quaternion rotation, out Vector3 result)
        {
            TransformWithoutOverlap(v, rotation, out var temp);
            result = temp;
        }

        /// <summary>
        /// 使用四元数变换向量。
        /// </summary>
        /// <param name="v">要转换的向量。</param>
        /// <param name="Rotation">应用于向量的旋转。</param>
        /// <return>转换后的向量。</return>
        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public static Vector3 Transform(in Vector3 v, in Quaternion rotation)
        {
            TransformWithoutOverlap(v, rotation, out var toReturn);
            return toReturn;
        }

        /// <summary>
        /// 使用四元数变换单位X方向。
        /// </summary>
        /// <param name="Rotation">应用于向量的旋转。</param>
        /// <param name="result">转换后的向量。</param>
        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public static void TransformUnitX(in Quaternion rotation, out Vector3 result)
        {
            // 此运算是v‘=q*v*q^-1的优化向下版本。
            // 展开后的形式将是将v视为"仅轴"四元数。
            // 并执行标准的四元数乘法。假设Q是归一化的,
            // q^-1可以用共轭来代替。
            float y2 = rotation.Y + rotation.Y;
            float z2 = rotation.Z + rotation.Z;
            float xy2 = rotation.X * y2;
            float xz2 = rotation.X * z2;
            float yy2 = rotation.Y * y2;
            float zz2 = rotation.Z * z2;
            float wy2 = rotation.W * y2;
            float wz2 = rotation.W * z2;
            result.X = 1f - yy2 - zz2;
            result.Y = xy2 + wz2;
            result.Z = xz2 - wy2;

        }

        /// <summary>
        /// 使用四元数变换单位Y向量。
        /// </summary>
        /// <param name="Rotation">应用于向量的旋转。</param>
        /// <param name="result">转换后的向量。</param>
        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public static void TransformUnitY(in Quaternion rotation, out Vector3 result)
        {
            // 此运算是v‘=q*v*q^-1的优化向下版本。
            // 展开后的形式将是将v视为"仅轴"四元数。
            // 并执行标准的四元数乘法。假设Q是归一化的,
            // q^-1可以用共轭来代替。
            float x2 = rotation.X + rotation.X;
            float y2 = rotation.Y + rotation.Y;
            float z2 = rotation.Z + rotation.Z;
            float xx2 = rotation.X * x2;
            float xy2 = rotation.X * y2;
            float yz2 = rotation.Y * z2;
            float zz2 = rotation.Z * z2;
            float wx2 = rotation.W * x2;
            float wz2 = rotation.W * z2;
            result.X = xy2 - wz2;
            result.Y = 1f - xx2 - zz2;
            result.Z = yz2 + wx2;
        }

        /// <summary>
        /// 使用四元数变换单位Z向量。
        /// </summary>
        /// <param name="Rotation">应用于向量的旋转。</param>
        /// <param name="result">转换后的向量。</param>
        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public static void TransformUnitZ(in Quaternion rotation, out Vector3 result)
        {
            // 此运算是v‘=q*v*q^-1的优化向下版本。
            // 展开后的形式将是将v视为"仅轴"四元数。
            // 并执行标准的四元数乘法。假设Q是归一化的,
            // q^-1可以用共轭来代替。
            float x2 = rotation.X + rotation.X;
            float y2 = rotation.Y + rotation.Y;
            float z2 = rotation.Z + rotation.Z;
            float xx2 = rotation.X * x2;
            float xz2 = rotation.X * z2;
            float yy2 = rotation.Y * y2;
            float yz2 = rotation.Y * z2;
            float wx2 = rotation.W * x2;
            float wy2 = rotation.W * y2;
            result.X = xz2 + wy2;
            result.Y = yz2 - wx2;
            result.Z = 1f - xx2 - yy2;
        }

        /// <summary>
        /// 从轴和角度创建四元数。
        /// </summary>
        /// <param name="axis">旋转轴。</param>
        /// <param name="angle">绕轴旋转的角度。</param>
        /// <return>表示轴和角度旋转的四元数。</return>
        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public static Quaternion CreateFromAxisAngle(in Vector3 axis, float angle)
        {
            double halfAngle = angle * 0.5;
            double s = Math.Sin(halfAngle);
            Quaternion q;
            q.X = (float)(axis.X * s);
            q.Y = (float)(axis.Y * s);
            q.Z = (float)(axis.Z * s);
            q.W = (float)Math.Cos(halfAngle);
            return q;
        }

        /// <summary>
        /// 从轴和角度创建四元数。
        /// </summary>
        /// <param name="axis">旋转轴。</param>
        /// <param name="angle">绕轴旋转的角度。</param>
        /// <param name="q">表示轴和角度旋转的四元数。</param>
        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public static void CreateFromAxisAngle(in Vector3 axis, float angle, out Quaternion q)
        {
            double halfAngle = angle * 0.5;
            double s = Math.Sin(halfAngle);
            q.X = (float)(axis.X * s);
            q.Y = (float)(axis.Y * s);
            q.Z = (float)(axis.Z * s);
            q.W = (float)Math.Cos(halfAngle);
        }

        /// <summary>
        /// 从偏航、俯仰和侧滚构造一个四元数。
        /// </summary>
        /// <param name="Yaw">旋转的偏航。</param>
        /// <param name="Pitch">旋转的间距。</param>
        /// <param name="roll">旋转滚动。</param>
        /// <return>表示偏航、俯仰和横摇的四元数。</return>
        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public static Quaternion CreateFromYawPitchRoll(float yaw, float pitch, float roll)
        {
            CreateFromYawPitchRoll(yaw, pitch, roll, out var toReturn);
            return toReturn;
        }

        /// <summary>
        /// 从偏航、俯仰和侧滚构造一个四元数。
        /// </summary>
        /// <param name="Yaw">旋转的偏航。</param>
        /// <param name="Pitch">旋转的间距。</param>
        /// <param name="roll">旋转滚动。</param>
        /// <param name="q">表示偏航、俯仰和侧滚的四元数。</param>
        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public static void CreateFromYawPitchRoll(float yaw, float pitch, float roll, out Quaternion q)
        {
            double halfRoll = roll * 0.5;
            double halfPitch = pitch * 0.5;
            double halfYaw = yaw * 0.5;

            double sinRoll = Math.Sin(halfRoll);
            double sinPitch = Math.Sin(halfPitch);
            double sinYaw = Math.Sin(halfYaw);

            double cosRoll = Math.Cos(halfRoll);
            double cosPitch = Math.Cos(halfPitch);
            double cosYaw = Math.Cos(halfYaw);

            double cosYawCosPitch = cosYaw * cosPitch;
            double cosYawSinPitch = cosYaw * sinPitch;
            double sinYawCosPitch = sinYaw * cosPitch;
            double sinYawSinPitch = sinYaw * sinPitch;

            q.X = (float)(cosYawSinPitch * cosRoll + sinYawCosPitch * sinRoll);
            q.Y = (float)(sinYawCosPitch * cosRoll - cosYawSinPitch * sinRoll);
            q.Z = (float)(cosYawCosPitch * sinRoll - sinYawSinPitch * cosRoll);
            q.W = (float)(cosYawCosPitch * cosRoll + sinYawSinPitch * sinRoll);

        }

        /// <summary>
        /// 计算由规格化四元数表示的角度更改。
        /// </summary>
        /// <param name="q">要转换的四元数。</param>
        /// <return>围绕由四元数表示的轴的角度。</return>
        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public static float GetAngleFromQuaternion(in Quaternion q)
        {
            float qw = Math.Abs(q.W);
            if (qw > 1)
                return 0;
            return 2 * (float)Math.Acos(qw);
        }

        /// <summary>
        /// 计算规格化四元数的轴角表示。
        /// </summary>
        /// <param name="q">要转换的四元数。</param>
        /// <param name="axis">轴由四元数表示。</param>
        /// <param name="angle">围绕由四元数表示的轴的角度。</param>
        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public static void GetAxisAngleFromQuaternion(in Quaternion q, out Vector3 axis, out float angle)
        {
            float qw = q.W;
            if (qw > 0)
            {
                axis.X = q.X;
                axis.Y = q.Y;
                axis.Z = q.Z;
            }
            else
            {
                axis.X = -q.X;
                axis.Y = -q.Y;
                axis.Z = -q.Z;
                qw = -qw;
            }

            float lengthSquared = axis.LengthSquared();
            if (lengthSquared > 1e-14f)
            {
                axis /= (float)Math.Sqrt(lengthSquared);
                angle = 2 * (float)Math.Acos(MathHelper.Clamp(qw, -1, 1));
            }
            else
            {
                axis = Vector3.UnitY;
                angle = 0;
            }
        }

        /// <summary>
        /// 计算两个规格化向量之间的四元数旋转。
        /// </summary>
        /// <param name="v1">第一个单位长度矢量。</param>
        /// <param name="v2">第二个单位长度矢量。</param>
        /// <param name="q">表示从v1到v2的旋转的四元数。</param>
        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public static void GetQuaternionBetweenNormalizedVectors(in Vector3 v1, in Vector3 v2, out Quaternion q)
        {
            float dot = Vector3.Dot(v1, v2);
            // 对于非法线向量,需要将轴长平方相乘：
            // Float w=dot+(Float)Math.Sqrt(v1.LengthSquared()*v2.LengthSquared());
            if (dot < -0.9999f) // 平行的、相反的方向
            {
                // 如果发生这种情况,所需的旋转角度约为180度。
                // 问题是我们可以选择任何垂直轴进行旋转。它的定义不是唯一的。
                // 解决方案是拾取任意垂直轴。
                // 投影到分量震级最低的平面上。
                // 在该2D平面上,执行90度旋转。
                float absX = Math.Abs(v1.X);
                float absY = Math.Abs(v1.Y);
                float absZ = Math.Abs(v1.Z);
                if (absX < absY && absX < absZ)
                    q = new Quaternion(0, -v1.Z, v1.Y, 0);
                else if (absY < absZ)
                    q = new Quaternion(-v1.Z, 0, v1.X, 0);
                else
                    q = new Quaternion(-v1.Y, v1.X, 0, 0);
            }
            else
            {
                var axis = Vector3.Cross(v1, v2);
                q = new Quaternion(axis.X, axis.Y, axis.Z, dot + 1);
            }
            Normalize(ref q);
        }

        // 下面的两个功能非常相似,但是用一个来表述另一个有点令人费解。
        // 两者兼而有之,简化了事情。

        /// <summary>
        /// 计算从开始方向到结束方向的旋转,使End=Quaternion.Concatenate(Start,Relative)。
        /// 假设两个输入参数都不与输出参数重叠。
        /// </summary>
        /// <param name="start">开始方向。</param>
        /// <param name="end">结束方向。</param>
        /// <param name="Relative">从起始方向到结束方向的相对旋转。</param>
        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public static void GetRelativeRotationWithoutOverlap(in Quaternion start, in Quaternion end, out Quaternion relative)
        {
            Conjugate(start, out var startInverse);
            ConcatenateWithoutOverlap(startInverse, end, out relative);
        }


        /// <summary>
        /// 将旋转变换到目标基础的局部空间,使Rotation=Quaternion.Concatenate(localRotation,targetBasis)
        /// 假设两个输入参数都不与输出参数重叠。
        /// </summary>
        /// <param name="Rotation">在原始参考框架中旋转。</param>
        /// <param name="targetBasis">将原始参照系中的旋转转换为。</param>
        /// <param name="localRotation">在目标基础的局部空间中旋转。</param>
        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public static void GetLocalRotationWithoutOverlap(in Quaternion rotation, in Quaternion targetBasis, out Quaternion localRotation)
        {
            Conjugate(targetBasis, out var basisInverse);
            ConcatenateWithoutOverlap(rotation, basisInverse, out localRotation);
        }
    }
}
